\section{控制}
\subsection{PID控制}
采用串级PID控制，内环控制$\theta$，外环控制$x$。式\eqref{eqlin2}的输入为$F$，输出为$\theta$传递函数为
\[G_1(s)=\frac{b}{s^2-a}\]
其中
\[a=\frac{Mg+mg}{Ml},b=\frac{1}{Ml}\]
取控制器$D(s)=K$，K足够大时即可使系统临界震荡。
\begin{figure}[!h]\begin{center}
    \begin{minipage}[b]{0.48\linewidth}\begin{center}
        \includegraphics[scale=0.4]{figbodeangle.pdf}
        \caption{角度PID控制伯德图}\label{fig4bodeangle}
    \end{center}\end{minipage}
    \hfill
    \begin{minipage}[b]{0.48\linewidth}\begin{center}
        \includegraphics[scale=0.4]{figbodeangle2.pdf}
        \caption{位置PID控制伯德图}\label{fig4bodeangle2}
    \end{center}\end{minipage}
\end{center}\end{figure}
从式\eqref{eqlin2}可看出其相位滞后180°常数，所以PID中不能加积分，PD控制器传递函数为
\begin{equation}\label{eqpidin}
    D_1(s)=\frac{(K_p+K_d)s+K_pN}{s+N}
\end{equation}
可见这是典型的超前网络。加上超前网络后相位恒大于$-180°$，所以性能(好像)几乎可以无限好。$K_p=K_d=10$的伯德图如图\ref{fig4bodeangle}所示。\par
外环的被控对象需要特别考虑。将内环角度控制整体作为被控对象，输入为参考指令角度$\theta_r$，输出为$\theta$和$x$。$\theta$和$x$满足式\eqref{eqans1}，线性化得
\[\ddot{x}=l\ddot{\theta}-g\theta\]
得到输入为$\theta$，输出为$x$的传递函数为
\[G(s)=l-\frac{g}{s^2}\]
于是输入为$F$，输出为$x$的传递函数为
\[G_2(s)=G(s)G_1(s)=\frac{(ls^2-g)b}{s^2(s^2-a)}\]
再算一下闭环传递函数，内环PID取$K_p=10$，$K_d=100$，由式\eqref{eqpidin}得到内环PID传递函数
\[D_1(s)=\frac{110s+1000}{s+100}\]
于是得到内环角度控制输入为$\theta_r$，输出为$x$的传递函数为
\[G_x(s)=\frac{D_1(s)G_2(s)}{1+D_1(s)G_2(s)}\]
取$K_p=K_i=1$的伯德图如图\ref{fig4bodeangle2}所示。(先别看)
整体结构框图如图\ref{fig4seriespid}所示。
\begin{figure}[!h]\begin{center}
    \hspace{-1cm}\includegraphics{seriespid.pdf}
    \caption{整体结构框图}\label{fig4seriespid}
\end{center}\end{figure}
传递函数形式过于复杂，而且听说好像稳定裕度只能在最小相位系统中才能分析，所以这个问题先留着。


\subsection{LQR控制}
Riccati方程
\begin{equation}\label{eqriccati}
    A^{T}P+PA+Q-PBR^{-1}B^TP=0
\end{equation}
为了便于求解，把被控对象分成角度和位移两个部分。角度的状态空间为
\[
    \begin{bmatrix}
        x_1' \\ x_2'
    \end{bmatrix}=\begin{bmatrix}
        0 & 1 \\ a & 0
    \end{bmatrix}\begin{bmatrix}
        x_1 \\ x_2
    \end{bmatrix}+\begin{bmatrix}
        0 \\ b
    \end{bmatrix}u
\]
代入Riccati方程\eqref{eqriccati}
\[
    \begin{bmatrix}
        ap_2 & ap_4 \\
        p_1 & p_2
    \end{bmatrix}+\begin{bmatrix}
        ap_2 & p_1 \\
        ap_4 & p_2
    \end{bmatrix}+\begin{bmatrix}
        q_1 & 0 \\
        0 & q_2
    \end{bmatrix}=\frac{1}{r}\begin{bmatrix}
        b^2p_2^2 & b^2p_2p_4 \\
        b^2p_2p_4 & b^2p_4^2
    \end{bmatrix}
\]
得到
\[
    p_2=\frac{ar+\sqrt{a^2r^2+b^2q_1}}{b^2},\quad
    p_4=\frac{\sqrt{(2p_2+q_2)r}}{b}
\]
\[u=-K\boldsymbol{x}=-\frac{b}{r}(p_2x_1+p_4x_2)\]
对位置和角度整体进行LQR控制时，取$Q=\text{diag}(1,0,1,0)$，$R=[0.1]$，使用schur分解的方法求解Riccati方程，参考资料见README.md。

